#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float32.h>
#include "communication/communication_plugin.h"
#include "smarteye_common/AllGVehiclesPos.h"
#include "smarteye_common/AllVehiclesPos.h"

#include <mavros_msgs/VFR_HUD.h>
#include <sensor_msgs/NavSatFix.h>

namespace commplugin {


class VehiclePosPubPlugin : public CommPlugin {
public:

  VehiclePosPubPlugin():
    vp_nh("~")
  {}

  void initialize(augmavconn::AugMavConnInterface::AugPtr station_link_, int target_system_id_, int target_component_id_)
  {
    vehicle_link = station_link_;

    uav_pos_pub = vp_nh.advertise<smarteye_common::AllVehiclesPos>("all_vehicles_pos", 10);
    gt_pos_pub = vp_nh.advertise<smarteye_common::AllGVehiclesPos>("all_gvehicles_pos", 10);

    my_pos_ll_sub = vp_nh.subscribe("/mavros/global_position/global", 10, &VehiclePosPubPlugin::set_lat_lon, this);
    my_pos_ra_sub = vp_nh.subscribe("/mavros/global_position/rel_alt", 10, &VehiclePosPubPlugin::set_rel_alt, this);
    my_pos_gs_sub = vp_nh.subscribe("/mavros/vfr_hud", 10, &VehiclePosPubPlugin::set_ground_speed, this);
    my_pos_cog_sub = vp_nh.subscribe("/mavros/global_position/raw/cog", 10, &VehiclePosPubPlugin::set_cog, this);

    vp_timer = vp_nh.createTimer(ros::Duration(0.05), &VehiclePosPubPlugin::send_pos, this);

    //vp_pub_timer = vp_nh.createTimer(ros::Duration(0.05), &VehiclePosPubPlugin::pub_self_pos, this);

  }


  const message_map get_rx_handlers() {
    return {
             MESSAGE_HANDLER(181, &VehiclePosPubPlugin::publish_uav_pos),
             MESSAGE_HANDLER(186, &VehiclePosPubPlugin::publish_gt_pos)
    };
  }


  void publish_uav_pos(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid){

    //mavlink_attitude_t att;
    mavlink_all_vehicles_pos_t uav_pos;
    //mavlink_msg_attitude_decode(msg, &att);
    mavlink_msg_all_vehicles_pos_decode(mmsg, &uav_pos);
    smarteye_common::AllVehiclesPos r_uav_pos;

    //drop self message from mavlink
    if (uav_pos.ID == vehicle_link->get_system_id())
    {
    	return;
    }

    r_uav_pos.Timestamp = uav_pos.Timestamp;
    r_uav_pos.lat = uav_pos.lat / 1E7;
    r_uav_pos.lon = uav_pos.lon / 1E7;
    r_uav_pos.alt = uav_pos.alt / 1E3;
    r_uav_pos.groundspeed = uav_pos.groundspeed;
    r_uav_pos.cog = uav_pos.cog / 100.0;
    r_uav_pos.ID = uav_pos.ID;
    r_uav_pos.header.stamp = ros::Time::now();
    uav_pos_pub.publish(r_uav_pos);
    //ROS_INFO("raw data lat: %d, lon: %d, alt: %d, ID: %d", uav_pos.lat, uav_pos.lon, uav_pos.alt, uav_pos.ID);

  }

  void publish_gt_pos(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid){

    //mavlink_attitude_t att;
    mavlink_packed_gvehicles_pos_t gt_pos;
    //mavlink_msg_attitude_decode(msg, &att);
    mavlink_msg_packed_gvehicles_pos_decode(mmsg,&gt_pos);
    // mavlink_msg_all_gvehicles_pos_decode(mmsg, &gt_pos);
    smarteye_common::AllGVehiclesPos r_gt_pos;
    for(int i= 0; i < 50 ; i++ ){
      r_gt_pos.data[i] = gt_pos.data[i];
    }
    // memcpy(r_gt_pos.data,(uint32_t *)gt_pos.data,50*sizeof(uint32_t));
    gt_pos_pub.publish(r_gt_pos);
  }




  void set_lat_lon(const sensor_msgs::NavSatFix::ConstPtr& g_pos)
  {
    my_pos_mavlink.lat = g_pos->latitude * 1E7;
    my_pos_mavlink.lon = g_pos->longitude * 1E7;

    my_pos_ros.lat = g_pos->latitude;
    my_pos_ros.lon = g_pos->longitude;

    LLIsSet = true;
  }

  void set_rel_alt(const std_msgs::Float64::ConstPtr& ra)
  {
    my_pos_mavlink.alt = ra->data * 1E3;
    my_pos_ros.alt = ra->data;

    RelAltIsSet = true;
  }


  void set_ground_speed(const mavros_msgs::VFR_HUD::ConstPtr& vfr)
  {
    my_pos_mavlink.groundspeed = vfr->groundspeed;
    my_pos_ros.groundspeed = vfr->groundspeed;

    GSpeedIsSet = true;
  }


  void set_cog(const std_msgs::Float32::ConstPtr& cog)
  {
    my_pos_mavlink.cog = cog->data * 1E2 ;

    my_pos_ros.cog = cog->data;

    CogIsSet = true;
  }

  //send my pos via mavlink, also publish on ros
  void send_pos(const ros::TimerEvent& event)
  {

//    mavlink_message_t msg;
//    my_pos_mavlink.ID = vehicle_link->get_system_id();
    my_pos_ros.ID = vehicle_link->get_system_id();
    //need to check the validity
    if (CogIsSet && LLIsSet && GSpeedIsSet && RelAltIsSet)
    {
//      mavlink_msg_all_vehicles_pos_encode_chan(
//            vehicle_link->get_system_id(),
//            vehicle_link->get_component_id(),
//            vehicle_link->get_channel(),
//            &msg,
//            &my_pos_mavlink);
//      vehicle_link->send_message(&msg);
      uav_pos_pub.publish(my_pos_ros);

      //ROS_INFO("msg sent");


    }
//    else
//    {
//      if (!CogIsSet) ROS_INFO("cog has not been set");
//      if (!LLIsSet) ROS_INFO("lat and lon has not been set");
//      if (!RelAltIsSet) ROS_INFO("rel_alt has not been set");
//      if (!GSpeedIsSet) ROS_INFO("ground speed has not been set");
//    }


  }


  void send_pos_test(const ros::TimerEvent& event){

    mavlink_message_t msg;
    mavlink_all_vehicles_pos_t test_uav_msg;

    mavlink_packed_gvehicles_pos_t test_target_msg;
    //mavlink_msg_attitude_decode(msg, &att);

    test_uav_msg.lat = 34 * 1E7;
    test_uav_msg.lon = 120 * 1E7;
    test_uav_msg.alt = 50 * 1E3;
    test_uav_msg.cog = 32112;
    test_uav_msg.groundspeed = 20.0;
    test_uav_msg.ID = 4;
    test_uav_msg.Timestamp = 11234;
    //need to check the validity

    mavlink_msg_all_vehicles_pos_encode_chan(
          vehicle_link->get_system_id(),
          vehicle_link->get_component_id(),
          vehicle_link->get_channel(),
          &msg,
          &test_uav_msg);
    vehicle_link->send_message(&msg);
    //ROS_INFO("test msg sent");
    test_target_msg.data[0] = 100;
    mavlink_msg_packed_gvehicles_pos_encode_chan(
          vehicle_link->get_system_id(),
          vehicle_link->get_component_id(),
          vehicle_link->get_channel(),
          &msg,
          &test_target_msg);
    vehicle_link->send_message(&msg);

  }

private:
  std::recursive_mutex mutex;
  ros::NodeHandle vp_nh;
  //UAV position pub, rev from mavlink
  ros::Publisher uav_pos_pub;

  //ground target position pub, rev from mavlink
  ros::Publisher gt_pos_pub;

  //my position sub: lat, lon, from "/mavros/global_position/global"
  ros::Subscriber my_pos_ll_sub;

  //my position sub: relative altitude, from "/mavros/global_position/rel_alt"
  ros::Subscriber my_pos_ra_sub;

  //my position sub: groundspeed, from "/mavros/global_position/vfr_hud"
  ros::Subscriber my_pos_gs_sub;

  //my position sub: cog, from "/mavros/global_position/vfr_hud"
  ros::Subscriber my_pos_cog_sub;

  mavlink_all_vehicles_pos_t my_pos_mavlink;
  smarteye_common::AllVehiclesPos my_pos_ros;


  augmavconn::AugMavConnInterface::AugPtr vehicle_link;
  //first arg: serial url
  ros::Timer vp_timer;

  //send self pos, 20hz
  ros::Timer vp_pub_timer;


  bool LLIsSet = false;
  bool RelAltIsSet = false;
  bool CogIsSet = false;
  bool GSpeedIsSet = false;



};
};	// namespace commplugin

PLUGINLIB_EXPORT_CLASS(commplugin::VehiclePosPubPlugin, commplugin::CommPlugin)
